﻿using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class BearingSensorLimitedAngle : Sensor
  {
    MapWithLandmarks.LandmarkIDs landmarkID;
    double headingDirection;
    double angleOfView;

    public BearingSensorLimitedAngle()
    {
      this.landmarkID = MapWithLandmarks.LandmarkIDs.centerLeftFlag;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 2.0;
    }

    public BearingSensorLimitedAngle(MapWithLandmarks.LandmarkIDs landmarkID, double headingDirection, double angleOfView)
    {
      this.landmarkID = landmarkID;
      this.headingDirection = headingDirection;
      this.angleOfView = angleOfView;
    }

    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      double sigma = Globals.bearingMeasurementSigma;
      double gaussianError = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double systematicError = 0.5;

      switch (Globals.selfLocatorProperties.SensorNoise)
      {
        case NoiseType.equal: break; //todo
        case NoiseType.noNoise: gaussianError = 0; systematicError = 0; break;
        case NoiseType.gaussianAndSystematic: break;
        case NoiseType.gaussianNoSystematic: systematicError = 0; break;
        case NoiseType.noGaussianButSystematic: gaussianError = 0; break;
      }

      AngleMeasurement measurement = (AngleMeasurement)PredictPerfect(worldState, map);
      if (measurement == null) return null;
      else return new AngleMeasurement(measurement.Angle + gaussianError + systematicError, this);
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      double angle = 0;
      
      double x_robot = 0;
      double y_robot = 0;
      double theta_robot = 0;
      double x_landmark = 0;
      double y_landmark = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (map is MapWithLandmarks)
      {
        x_landmark = (map as MapWithLandmarks).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithLandmarks).YPositionOfLandmark[(int)landmarkID];
      }
      if (map is MapWithSingleLandmark)
      {
        x_landmark = (map as MapWithSingleLandmark).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithSingleLandmark).YPositionOfLandmark[(int)landmarkID];
      }

      angle = Math.Atan2(y_landmark - y_robot, x_landmark - x_robot) - theta_robot;

      if (Math.Abs(MathTools.Normalize(angle - headingDirection)) < angleOfView / 2.0)
        return new AngleMeasurement(angle, this);
      else
        return new AngleMeasurement(headingDirection + Math.PI, this);
    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      AngleMeasurement angleMeasurement = (PredictPerfect(worldState, map) as AngleMeasurement);
      if (measurement != null)
      {
        double expectedMeasurement = angleMeasurement.Angle;
        double conductedMeasurement = (measurement as AngleMeasurement).Angle;
        return Globals.Gaussian(conductedMeasurement, Globals.bearingMeasurementSigma * 20, expectedMeasurement);
      }
      else
        return 0;
    }

/*
    public override void  Draw(System.Drawing.Graphics graphics, Measurement measurement, WorldState worldState, double shade)
    {
      double startX = 0;
      double startY = 0;
      double endX = 0;
      double endY = 0;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      endX = startX + Math.Cos((measurement as AngleMeasurement).Angle) * 500.0;
      endY = startY + Math.Sin((measurement as AngleMeasurement).Angle) * 500.0;

      Color color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
      Pen pen;
      if (shade == 0.9999)
        pen = new Pen(color, 1);
      else
        pen = new Pen(color, 3);
      graphics.DrawLine(pen, (float)startX, (float)startY, (float)endX, (float)endY);
    }
*/
    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      //TODO
      throw new Exception("The method or operation is not implemented.");
    }

    public override string ToString()
    {
      throw new Exception("The method or operation is not implemented.");
    }
  }
}
